#include "pathfinder.h"

namespace poly
{
pathfinder::pathfinder()
{
}
void pathfinder::begin(float3 object, float3 destination, std::vector<Polygon> obstacle)
{
	m_ObjectDestination = destination;
	m_ObstacleVector    = obstacle;
	m_ObjectCenter      = object;
	m_LineDestObject.resize(2);
}
float3 pathfinder::find(){
		//Update the line that goes from object center to destination
		m_LineDestObject[0] = m_ObjectDestination;
		m_LineDestObject[1] = m_ObjectCenter;

		ContactList tempContact;

		bool moveToDest = true;

		//Initial check to see if destination is visible at all
		if(destinationVisible(m_ObjectCenter))
			m_ObjectMoveTo = m_ObjectDestination;
		else
			moveToDest = false;

		//If the object is not moving to destination, initiate pathfinding
		if(!moveToDest)
		{
			poly::PolyLine lineToVertex; //Line from object center to each vertex
			lineToVertex.resize(2);
			m_ObjectMoveTo = m_ObjectCenter;

			std::vector<float3> candidVertices;

			for(std::vector<Polygon>::iterator Obstacle = m_ObstacleVector.begin();
					Obstacle < m_ObstacleVector.end();
					Obstacle++)
			{
				for(std::vector<float3>::iterator Vertex = Obstacle->begin();
					Vertex < Obstacle->end();
					Vertex++)
				{
					lineToVertex[0] = m_ObjectCenter;
					lineToVertex[1] = *Vertex;
					intersect(tempContact, *Obstacle, lineToVertex);
					
					//Isolated vertex-eye line test.
					if(tempContact.size() == 0)
					{
						poly::PolyLine lineToVertex2; //Line from object center to vertex
						lineToVertex2.push_back(m_ObjectCenter);
						lineToVertex2.push_back(*Vertex);
						bool visibleToEye = true;


						//Generalized vertex-eye line test (includes other polygons too)
						if(visibleToEye)
						{
							for(std::vector<Polygon>::iterator Obstacle2 = m_ObstacleVector.begin();
								Obstacle2 < m_ObstacleVector.end();
								Obstacle2++)
							{
								intersect(tempContact, *Obstacle2, lineToVertex2);
								if(tempContact.size() != 0)
								{
									visibleToEye = false;
									break;
								}
							}
						}

						//Isolated vertex-destination length test
						if(visibleToEye)
						{
							if(length((*Vertex) - m_ObjectDestination) < length(m_ObjectDestination - m_ObjectMoveTo))
							{
								m_ObjectMoveTo = (*Vertex);
							}
						}
					}
				}
				
				candidVertices.push_back(m_ObjectMoveTo);
				m_ObjectMoveTo = m_ObjectCenter;
			}

			//Generalized candidate vertex-destination length test
			for(int i = 0; i < candidVertices.size(); i++)
			{
				if(length(candidVertices[i] - m_ObjectDestination) < length(m_ObjectDestination - m_ObjectMoveTo))
				{
					m_ObjectMoveTo = candidVertices[i];
				}
			}
			
			lineToVertex.clear();
		}

		//Updates the object's future center (the next point on the path) in the system
		m_ObjectCenter = m_ObjectMoveTo;

		return m_ObjectCenter;
}
bool pathfinder::destinationVisible(float3 objectPosition)
{
	PolyLine m_LineDestObject2;
	m_LineDestObject2.resize(2);
	m_LineDestObject2[0] = m_ObjectDestination;
	m_LineDestObject2[1] = objectPosition;

	ContactList tempContact;

	for(int i=0; i<m_ObstacleVector.size(); i++)
		{
			intersect(tempContact,m_ObstacleVector[i], m_LineDestObject2);

			//If destination view is obstructed by any of the obstacles
			//Then object is not going to move to destination.
			if(tempContact.size() != 0)
			{
				return false;
			}
		}

	return true;

}
float3 pathfinder::next()
{
	float3 nextPosition = find();
	return nextPosition;
}
float3 pathfinder::next(float3 objectPosition)
{
	m_ObjectCenter = objectPosition;
	return find();
}
void pathfinder::end()
{
	m_LineDestObject.clear();

}

}//end of poly